#include "pubRobotStatus.hpp"
#include "rclcpp/rclcpp.hpp"
#include "chrono"
constexpr std::chrono::seconds timeout_{10};
using namespace std::chrono_literals;
namespace hnurm_behavior_trees
{

using std::placeholders::_1;
// using std::placeholders::_2;

pubRobotStatus::pubRobotStatus(
  const std::string & name,
  const BT::NodeConfiguration & conf)
: BT::SyncActionNode(name, conf)
{
  node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
  callback_group_ = node_->create_callback_group(
    rclcpp::CallbackGroupType::MutuallyExclusive,
    false);
  callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());

  
  recv_topic_ = node_->declare_parameter("recv_topic","/vision_recv_data");
  back_target_ = node_->declare_parameter("back_target","/back_target");

  send_topic_ = node_->declare_parameter("send_topic","/vision_send_topic");
  target_topic_ = node_->declare_parameter("target_topic","/target");
  global_position_ = node_->declare_parameter("global_position_","/global_costmap/published_footprint");
  pursuit_armor_id_ = node_->declare_parameter("pursuit_armor_id","all");

  decision_send_topic_ = node_->declare_parameter("decision_send_topic","/decision/vision_send_topic");
  odom_topic_ = node_->declare_parameter("odom_topic","/Odometry_transformed");
  hp_threshold = node_->declare_parameter("hp_threshold",100.0);
  node_->declare_parameter<std::vector<std::string>>("goal_points", std::vector<std::string>());
  auto goal_points = node_->get_parameter("goal_points").as_string_array();  
  // decode goal points
    for (const auto& point : goal_points) {
      current_global_pose gp;
      size_t comma_pos = point.find(',');
      gp.pose_x = std::stod(point.substr(0, comma_pos));
      gp.pose_y = std::stod(point.substr(comma_pos + 1));
      cruise_goals_.push_back(gp);
    }
  // print goal points 
  for (const auto& goal : cruise_goals_) {
    RCLCPP_INFO(node_->get_logger(), "Loaded goal: (%.2f, %.2f)", 
                goal.pose_x, goal.pose_y);
  }


  RCLCPP_INFO(node_->get_logger(), "decision is running");

  // rclcpp::QoS qos(rclcpp::KeepLast(1));
  // qos.best_effort().durability_volatile();   //pay attention to t
  // // auto qos  = rclcpp::SystemDefaultsQoS();

  rclcpp::SubscriptionOptions sub_option;
  sub_option.callback_group = callback_group_;
  recv_sub_ = node_->create_subscription<hnurm_interfaces::msg::VisionRecvData>(
    recv_topic_,
    rclcpp::SensorDataQoS(),
    std::bind(&pubRobotStatus::recv_callback, this, _1),
    sub_option);
  back_target_sub_ = node_->create_subscription<std_msgs::msg::Float32>(
    back_target_,
    rclcpp::SensorDataQoS(),
    std::bind(&pubRobotStatus::back_target_callback, this, _1),
    sub_option);
  send_sub_ = node_->create_subscription<hnurm_interfaces::msg::VisionSendData>(
    send_topic_,
    rclcpp::ServicesQoS(),
    std::bind(&pubRobotStatus::send_callback, this, _1),
    sub_option);
  
  target_sub_ = node_->create_subscription<hnurm_interfaces::msg::Target>(
    target_topic_,
    rclcpp::SensorDataQoS(),
    std::bind(&pubRobotStatus::target_callback, this, _1),
    sub_option);

  global_pose_sub_= node_->create_subscription<geometry_msgs::msg::PolygonStamped>(
    global_position_,
    rclcpp::SensorDataQoS(),
    std::bind(&pubRobotStatus::global_pose_callback, this, _1),
    sub_option);
  odom_sub_= node_->create_subscription<nav_msgs::msg::Odometry>(
    odom_topic_,
    rclcpp::SensorDataQoS(),
    std::bind(&pubRobotStatus::odom_callback, this, _1),
    sub_option);
  
  timer_ = node_->create_wall_timer(10ms, std::bind(&pubRobotStatus::timer_callback, this),callback_group_);
  tf_listener_timer_ = node_->create_wall_timer(1000ms, std::bind(&pubRobotStatus::tf_listener_callback, this),callback_group_);
  decision_pub_ = node_->create_publisher<hnurm_interfaces::msg::VisionSendData>("/decision/vision_send_data", rclcpp::SensorDataQoS());

  pursuit_pub_ = node_->create_publisher<geometry_msgs::msg::PoseStamped>("goal_update", 10);
  // 需要对tfbuffer 和listener 进行初始化，不然会段错误
  
  tf_buffer_ = std::make_unique<tf2_ros::Buffer>(node_->get_clock());
  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

  // executor_thread_ = std::thread([this]() {
  //       while (rclcpp::ok()) {
  //           callback_group_executor_.spin_all(std::chrono::milliseconds(1));
  //           callback_group_executor_.spin_all(std::chrono::milliseconds(19));  
  //           // callback_group_executor_.spin_some();
  //       }
  //   });
  supply_goal.header.stamp = node_->now(); // 设置时间戳为当前时间
  supply_goal.header.frame_id = "map";    // 设置坐标系 ID
  supply_goal.pose.position.x = -0.50;
  supply_goal.pose.position.y = 0.50;
  supply_goal.pose.position.z = 0.0;
  supply_goal.pose.orientation.x = 0;
  supply_goal.pose.orientation.y = 0;
  supply_goal.pose.orientation.z = 0;
  supply_goal.pose.orientation.w = 1;

}
pubRobotStatus::~pubRobotStatus() {
    if (executor_thread_.joinable()) {
        executor_thread_.join();
    }
}


/**************FSM TO CONTROL BT********************/

BT::NodeStatus pubRobotStatus::tick()
{ 
  if (supply_)     //加血逻辑为最高优先级
  {
    setOutput("hp_status",true);
    setOutput("supply_goal",supply_goal);
    all_off();
  }
  else 
  {
    setOutput("hp_status",false);
    if (pursuit_)
    { 
       RCLCPP_WARN(node_->get_logger(), "Pursuit mode on");
      setOutput("cruise_status",false);
      setOutput("pursuit_status",true);
      if (!stay_)
      {
        setOutput("pursuit_mode1",true);
        setOutput("pursuit_mode2",false);
      }
      else{
        setOutput("pursuit_mode1",false);
        setOutput("pursuit_mode2",true);
      }
    }
    else 
    {
      RCLCPP_WARN(node_->get_logger(), "Cruise mode on");
      setOutput("cruise_status",true);
      setOutput("pursuit_status",false);
      setOutput("cruise_goal",update_goal(cruise_goals_[cruise_iterator.idx]));
    }
  }
  

    callback_group_executor_.spin_some();

  return BT::NodeStatus::SUCCESS;  //每次tick直接返回success，作为一定频率更新的global status publisher
}

/**********************************/

void pubRobotStatus::all_off()
{
  setOutput("pursuit_status",false);
  setOutput("cruise_status",false);
}

bool pubRobotStatus::goal_checker(current_global_pose cruise_goal,current_global_pose current_pose){
  if (std::sqrt(std::pow(current_pose.pose_x - cruise_goal.pose_x, 2) + std::pow(current_pose.pose_y - cruise_goal.pose_y, 2))<0.5)
  {
    return true;
    // RCLCPP_WARN(node_->get_logger(), "Goal reached ");

  }
  return false;
}

void pubRobotStatus::tf_listener_callback()
{ 
    std::lock_guard<std::mutex> lock(transform_mutex);
        try {
            transform_m2bf = tf_buffer_->lookupTransform(
                "map",          // 目标坐标系
                "base_footprint",    // 源坐标系
                rclcpp::Time(0), // 时间戳（0表示最新）
                rclcpp::Duration::from_seconds(0.1)  // 超时时间
            );
            RCLCPP_ERROR(node_->get_logger(), "Transform found!");
        } catch (tf2::TransformException &ex) {
            RCLCPP_ERROR(node_->get_logger(), "Transform failed: %s. Waiting for transform...", ex.what());
            transform_m2bf = geometry_msgs::msg::TransformStamped();  // 重置为默认值
            return;  // 如果转换查找失败，则退出该回调
        }
}

geometry_msgs::msg::PoseStamped pubRobotStatus::update_goal(current_global_pose cruise_goal){
  geometry_msgs::msg::PoseStamped pose;
    pose.header.stamp = node_->now(); // 设置时间戳为当前时间
    pose.header.frame_id = "map";    // 设置坐标系 ID
    pose.pose.position.x = cruise_goal.pose_x;
    pose.pose.position.y = cruise_goal.pose_y;
    pose.pose.position.z = 0.0;
    pose.pose.orientation.x = 0;
    pose.pose.orientation.y = 0;
    pose.pose.orientation.z = 0;
    pose.pose.orientation.w = 1;
    // pursuit_pub_->publish(pose);
    return pose;
}


void pubRobotStatus::recv_callback(const hnurm_interfaces::msg::VisionRecvData::SharedPtr msg)
{
  referee_status_.current_hp = msg->current_hp;
  if (msg->current_hp<hp_threshold)
  {
    supply_ = true;
  }
  else if(msg->current_hp>=360.0) 
  {
    supply_ = false;  //开局默认为false，只有当血量第一次降低到阈值以下才会转变为true，后续需要加血到360.0以上才恢复false
  }

  referee_status_.rpy.roll = msg->roll;
  referee_status_.rpy.pitch = msg->pitch;
  referee_status_.rpy.yaw = msg->yaw;
  
}

/*
当前进程占用云台的运动，当转到背后的过程中，如果主相机检测到了目标呢？ --->以主相机为主，终止发布云台控制消息，将控制权交还主相机。

*/
int counter = 0;
void pubRobotStatus::timer_callback()  //此处实现全向感知相机的进程分配
{
  // 检查当前控制权是否属于后视相机
  hnurm_interfaces::msg::VisionSendData send;
  rclcpp::Time now = node_->now();

  if (current_authority.load(std::memory_order_acquire) == ControlAuthority::MainCamera) {
      // std::cout<<"main camera obtained control !"<<std::endl;
      back_camera_control.on_control = false;   //reset
      return;  // 控制权已被抢占，不执行后续操作
  } 
  if(!send_data.target_state&&back_camera_control.on_control)  //当主相机没有检测到，并且后视相机检测到了
  {
    // RCLCPP_INFO(node_->get_logger(), "back camera obtained control");
 
    send.header.stamp = now;
    send.header.frame_id = "serial_send";
    send.target_state.data = hnurm_interfaces::msg::TargetState::CONVERGING;    //send converging state
    send.yaw = back_camera_control.reference_yaw_ -45.0 + (back_target_bias);   //减小转动的幅度，靠惯性转过去
    // if (std::abs(back_camera_control.reference_yaw_ - referee_status_.rpy.yaw)<10.0)
    // {
    //   back_camera_control.on_control = false;
    //   return;
    // }
    // decision_pub_->publish(send);  
    }
}

void pubRobotStatus::odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg)
{ 
  // current_pose_.pose_x = msg->pose.pose.position.x - transform_m2bf.translation.x;
  // current_pose_.pose_y = msg->pose.pose.position.y - transform_m2bf.translation.y;
  (void)msg;
}


void pubRobotStatus::back_target_callback(const std_msgs::msg::Float32::SharedPtr msg)
{
  // back_target_bias = msg->data;   //接收到的数据>200.0的时表示此时back camera 没有识别到目标,在tf_publisher里面控制限制了数据区间
  // 更新滑动窗口
  if (msg->data !=666.0)
  {
    if (!back_camera_control.on_control)
    {
      back_target_bias = msg->data;
    }
  }
        
  bias_window_.push_back(msg->data);
  if (bias_window_.size() > WINDOW_SIZE) {
      bias_window_.pop_front();
  }
  // 计算均值
  float filtered_bias = 0.0f;
  for (auto val : bias_window_) {
      filtered_bias += val;
  }
  filtered_bias /= bias_window_.size();

  if(filtered_bias < 666.0)   
  { 
  // }
     // 尝试申请控制权（仅在当前空闲或自身已持有控制权时）
    ControlAuthority expected = ControlAuthority::Idle;
    if (current_authority.compare_exchange_strong(
            expected, 
            ControlAuthority::BackCamera,
            std::memory_order_acq_rel
        )) {
            // 申请成功，启动控制逻辑
            // RCLCPP_INFO(node_->get_logger(), "back camera obtained control");
            back_camera_control.reference_yaw_ = referee_status_.rpy.yaw;
            back_camera_control.on_control = true;
        }
    else back_camera_control.on_control = false;
  }
}

void pubRobotStatus::send_callback(const hnurm_interfaces::msg::VisionSendData::SharedPtr msg)
{
  if (msg->target_state.data>=1)    //利用是否识别到控制是否追击和巡逻
  {
    pursuit_ = true;
    cruise_ = false;
  }
  else{
    pursuit_ = false;
    cruise_ = true;
  }

  send_data.target_state = (msg->target_state.data >0 ) ? true:false;    //recv vision data
  if(send_data.target_state)  //主相机获取控制权
  {
    // RCLCPP_INFO(node_->get_logger(), "main camera obtained control");
    current_authority.store(ControlAuthority::MainCamera, std::memory_order_release);
    back_camera_control.on_control = false;
  }
  else 
  {
    //释放主相机控制权
    current_authority.store(ControlAuthority::Idle, std::memory_order_release);
    // //释放后视相机正在执行控制状态
    // back_camera_control.on_control = false;
  }
  send_data.target_distance = msg->target_distance;     // used for calculate target nav point
}


pubRobotStatus::current_global_pose pubRobotStatus::find_Away_point(float yaw,float distance)
{
  const float target_x_base = distance * cos(yaw);
  const float target_y_base = distance * sin(yaw);     
  // 转换到map坐标系
  const float target_x_map = current_pose_.pose_x + target_x_base;
  const float target_y_map = current_pose_.pose_y + target_y_base;

  // 目标到机器人的方向向量
  const float dx = current_pose_.pose_x  - target_x_map;
  const float dy = current_pose_.pose_y  - target_y_map;
  const float direction_norm = sqrt(dx*dx + dy*dy);

  // 单位向量
  const float unit_dx = dx / direction_norm;
  const float unit_dy = dy / direction_norm;
  // 期望保持的距离
  const float desired_dist = 3.4;

  // 计算期望点坐标（从目标点向机器人方向后退）
  const float desired_x = target_x_map + unit_dx * (direction_norm - desired_dist);
  const float desired_y = target_y_map + unit_dy * (direction_norm - desired_dist);
  current_global_pose target_pose_{desired_x,desired_y};
  return target_pose_;
}

void pubRobotStatus::target_callback(const hnurm_interfaces::msg::Target::SharedPtr msg) 
{   
   RCLCPP_WARN(node_->get_logger(), "DETECTED !!!");
   // 获取机器人朝向（yaw角）
    tf2::Quaternion q(
        transform_m2bf.transform.rotation.x,
        transform_m2bf.transform.rotation.y,
        transform_m2bf.transform.rotation.z,
        transform_m2bf.transform.rotation.w
    );
    tf2::Matrix3x3 m(q);
    double roll, pitch, yaw;
    m.getRPY(roll, pitch, yaw);
  if(send_data.target_distance<2.6) //发布远离点  //detect到了，但是还没有更新到最新的target_distance
  {
    // current_global_pose pub_pose = find_Away_point(yaw,send_data.target_distance);
    // geometry_msgs::msg::PoseStamped pose;
    // pose.header = msg->header;
    // pose.header.frame_id = "map";
    // pose.pose.position.x =  pub_pose.pose_x;
    // pose.pose.position.y = pub_pose.pose_y;
    // pose.pose.position.z = 0.0;
    // pose.pose.orientation.x = 0;
    // pose.pose.orientation.y = 0;
    // pose.pose.orientation.z = 0;
    // pose.pose.orientation.w = 1;
    // pursuit_pub_ ->publish(pose); 
    // pursuit_type.store(PursuitType::Away);
    stay_ = true;   

  }
  else{
      float pub_x = send_data.target_distance*std::cos(yaw)  + current_pose_.pose_x;
      float pub_y = send_data.target_distance*std::sin(yaw) + current_pose_.pose_y;
      geometry_msgs::msg::PoseStamped pose;
      pose.header = msg->header;
      pose.header.frame_id = "map";
      pose.pose.position.x =  pub_x;
      pose.pose.position.y = pub_y;
      pose.pose.position.z = 0.0;
      pose.pose.orientation.x = 0;
      pose.pose.orientation.y = 0;
      pose.pose.orientation.z = 0;
      pose.pose.orientation.w = 1;
      pursuit_pub_ ->publish(pose); 
      pursuit_type.store(PursuitType::Approach);
      stay_ = false;
  }
}


void pubRobotStatus::global_pose_callback(const geometry_msgs::msg::PolygonStamped::SharedPtr msg)
{   
    current_pose_.pose_x = msg->polygon.points[1].x;    //取published_footprint（数组中储存32个）中的一个作为当前位置的估计即可，后续如果有抖动的话再修改别的
    current_pose_.pose_y = msg->polygon.points[1].y;

     RCLCPP_INFO(node_->get_logger(),"current_point: x=%f ,y=%f ,cruise_point: x=%f ,y=%f ",current_pose_.pose_x,current_pose_.pose_y,cruise_goals_[cruise_iterator.idx].pose_x,cruise_goals_[cruise_iterator.idx].pose_y);

  // cruise_iterator.former_is_reached = goal_checker(cruise_goals_[cruise_iterator.idx], current_pose_);
  // if (cruise_iterator.former_is_reached) {
      
  //     cruise_iterator.idx++;
  //     if (static_cast<std::size_t>(cruise_iterator.idx) >= cruise_goals_.size()) {
  //         cruise_iterator.idx = 0;
  //     }
  // }
  const bool is_reached = goal_checker(cruise_goals_[cruise_iterator.idx], current_pose_);
  if (is_reached && !is_waiting_) {
    // 首次到达目标点，触发等待
    wait_start_time_ = node_->now();
    is_waiting_ = true;
    RCLCPP_INFO(node_->get_logger(), "cruise goal is reached ,start waiting %.1f seconds...", wait_duration_);
  }
  if (!is_reached && !is_waiting_)
  {
    timeout_start_time_ = node_ ->now();
    const auto elapsed_timeout_ = node_->now() - timeout_start_time_;
    if(elapsed_timeout_.seconds()>=timeout_)
    {
      //timeout update goal 
      cruise_iterator.idx++;
      if (static_cast<std::size_t>(cruise_iterator.idx) >= cruise_goals_.size()) {
          cruise_iterator.idx = 0;
      }
      is_waiting_ = false;
      RCLCPP_INFO(node_->get_logger(), "time out ,ahead to the [IDX=%d]", cruise_iterator.idx);
    }
  }
  
  if(is_waiting_)
  {
    const auto elapsed = node_->now() - wait_start_time_;
    if(elapsed.seconds()>=wait_duration_)
    {
      //finished waiting 
      cruise_iterator.idx++;
      if (static_cast<std::size_t>(cruise_iterator.idx) >= cruise_goals_.size()) {
          cruise_iterator.idx = 0;
      }
      is_waiting_ = false;
      RCLCPP_INFO(node_->get_logger(), "finished waiting ,ahead to the [IDX=%d]", cruise_iterator.idx);
    }
  }
  cruise_iterator.former_is_reached = is_reached && !is_waiting_;
}

}  // namespace nav2_behavior_tree

#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory)
{
  factory.registerNodeType<hnurm_behavior_trees::pubRobotStatus>("pubRobotStatus");
}
